"""
Env 2D
@author: Gang Xu
"""
import os
import yaml
import json
import random
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rcParams
from shapely.geometry import Polygon, Point
from rvo.poly_obstacle import PolyObstacle
from draw.plt2d import draw_polygon_2d


class Env:
    def __init__(self, env_type, is_grid_map=False):
        self.xDim = 501  # size of background
        self.yDim = 501
        self.env_type = env_type
        self.is_grid_map = is_grid_map
        self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
                        (1, 0), (1, -1), (0, -1), (-1, -1)]
        self.poly_obs, self.obs_grid = self.read_obs_map()

    def read_obs_map(self):
        if self.env_type == "small":
            if self.is_grid_map:
                return [], self.obs_test_grid()
            else:
                return self.small_map(), []
        elif self.env_type == "large_maze":
            if self.is_grid_map:
                return [], self.poly_maze_map_grid()
            else:
                return self.poly_maze_map(), []
        elif self.env_type == "large":
            if self.is_grid_map:
                return [], self.obs_map_grid()
            else:
                return self.poly_obs_map(), []
        elif self.env_type == "large_corridor":
            if self.is_grid_map:
                return [], self.read_json()
            else:
                return self.corridor_map(), []
        elif self.env_type == "symmetric":
            if self.is_grid_map:
                return [], self.symmetric_grid()
            else:
                return self.symmetrical_map(), []
        elif self.env_type == "large_obs":
            if self.is_grid_map:
                return [], self.large_obs_grid()
            else:
                return self.large_obs_map(), []
        elif self.env_type == "small_indoor":
            if self.is_grid_map:
                return [], self.small_indoor_grid()
            else:
                return self.small_indoor_map(), []
        else:
            if self.is_grid_map:
                return [], self.obs_test_grid()
            else:
                return [], self.obs_test_grid()

    def obs_test_grid(self):
        """
        小范围经典的测试场景，栅格绘制, 51*31
        """
        self.xDim, self.yDim = 50, 30
        x, y = self.xDim + 1, self.yDim + 1
        obstacles = set()
        poly_gons = self.small_map()

        # boundary of environment
        for i in range(self.xDim):
            obstacles.add((i, 0))
            obstacles.add((i, self.yDim - 1))
        for i in range(self.yDim):
            obstacles.add((0, i))
            obstacles.add((self.xDim - 1, i))

        # obstacles
        for i in range(4, len(poly_gons)):
            for j in range(len(poly_gons[i].vertices_pos)):
                point = poly_gons[i].vertices_[j].point_
                next_point = poly_gons[i].vertices_[j].next_.point_
                if point[0] == next_point[0]:
                    min_y = min(point[1], next_point[1])
                    max_y = max(point[1], next_point[1])
                    for k in range(min_y, max_y + 1):
                        p = (point[0], k)
                        if p not in obstacles:
                            obstacles.add(p)
                elif point[1] == next_point[1]:
                    min_x = min(point[0], next_point[0])
                    max_x = max(point[0], next_point[0])
                    for k in range(min_x, max_x + 1):
                        p = (k, point[1])
                        if p not in obstacles:
                            obstacles.add(p)
        return obstacles

    def poly_maze_map_grid(self):
        """
        大范围的二维迷宫场景，栅格绘制, 500*500
        """
        self.xDim, self.yDim = 500, 500
        x, y = self.xDim + 1, self.yDim + 1
        poly_maze = self.poly_maze_map()  # 大范围的二维迷宫场景，多边形绘制, 501m*501m
        obstacles = set()

        # boundary of environment
        for i in range(x):
            obstacles.add((i, 0))
            obstacles.add((i, y - 1))
        for i in range(y):
            obstacles.add((0, i))
            obstacles.add((x - 1, i))

        # obstacles
        for i in range(4, len(poly_maze)):
            for j in range(len(poly_maze[i].vertices_pos)):
                point = poly_maze[i].vertices_[j].point_
                next_point = poly_maze[i].vertices_[j].next_.point_
                if point[0] == next_point[0]:
                    min_y = min(point[1], next_point[1])
                    max_y = max(point[1], next_point[1])
                    for k in range(min_y, max_y + 1):
                        p = (point[0], k)
                        if p not in obstacles:
                            obstacles.add(p)
                elif point[1] == next_point[1]:
                    min_x = min(point[0], next_point[0])
                    max_x = max(point[0], next_point[0])
                    for k in range(min_x, max_x + 1):
                        p = (k, point[1])
                        if p not in obstacles:
                            obstacles.add(p)
        return obstacles

    def symmetric_grid(self):
        """
        大范围的对称二维场景，栅格绘制，801*201
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 800, 200
        return self.read_json(name='/symetric_env.json')

    def obs_map_grid(self):
        """
        大范围的二维场景，栅格绘制，681*501
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 680, 500
        x, y = self.xDim + 1, self.yDim + 1
        obstacles = set()
        poly_gons = self.poly_obs_map()

        # boundary of environment
        for i in range(x):
            obstacles.add((i, 0))
            obstacles.add((i, y - 1))
        for i in range(y):
            obstacles.add((0, i))
            obstacles.add((x - 1, i))

        # obstacles
        for i in range(4, len(poly_gons)):
            for j in range(len(poly_gons[i].vertices_pos)):
                point = poly_gons[i].vertices_[j].point_
                next_point = poly_gons[i].vertices_[j].next_.point_
                if point[0] == next_point[0]:
                    min_y = min(point[1], next_point[1])
                    max_y = max(point[1], next_point[1])
                    for k in range(min_y, max_y + 1):
                        p = (point[0], k)
                        if p not in obstacles:
                            obstacles.add(p)
                elif point[1] == next_point[1]:
                    min_x = min(point[0], next_point[0])
                    max_x = max(point[0], next_point[0])
                    for k in range(min_x, max_x + 1):
                        p = (k, point[1])
                        if p not in obstacles:
                            obstacles.add(p)
        return obstacles

    def large_obs_grid(self):
        """
        大范围的对称二维场景，栅格绘制，6001*5001
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 6000, 5000

        # obstacles = set()
        # poly_gons = self.large_obs_map()
        # blocks = []
        # for x in range(0, self.xDim+1, 5):
        #     for y in range(0, self.yDim+1, 5):
        #         for ob in poly_gons:
        #             polygon = Polygon(ob.vertices_pos)
        #             point = Point((x, y))
        #             if point.within(polygon) or point.touches(polygon):
        #                 obstacles.add((x, y))
        #                 blocks.append([x, y])
        # self.write_env_cfg(blocks, name='/large_obs_env.json')

        obstacles = self.read_json(name='/large_obs_env.json')
        return obstacles

    def small_indoor_grid(self):
        """
        大范围的对称二维场景，栅格绘制，6001*5001
        Initialize obstacles' positions
        :return: map of obstacles
        """
        # obstacles = set()
        # poly_gons = self.small_indoor_map()
        # self.xDim, self.yDim = 601, 501
        # res = 0.01
        # blocks = []
        # for x in range(0, self.xDim, 1):
        #     for y in range(0, self.yDim, 1):
        #         for ob in poly_gons:
        #             polygon = Polygon(ob.vertices_pos)
        #             point = Point((x*res, y*res))
        #             if point.within(polygon) or point.touches(polygon):
        #                 blocks.append([x*res, y*res])
        # self.write_env_cfg(blocks, name='/small_indoor_grid.json')

        obstacles = self.read_json(name='/small_indoor_grid.json')
        return obstacles

    def rect_vertices(self, min_x, min_y, max_x, max_y):
        max_x = min(max_x, self.xDim)
        max_y = min(max_y, self.yDim)
        rect_vertices = [[min_x, min_y], [max_x, min_y], [max_x, max_y], [min_x, max_y]]
        return rect_vertices

    def large_obs_map(self):
        self.xDim, self.yDim = 6000, 5000
        obstacles = []
        obstacle_radius = 50
        ob_pos = [[3823.46, 3892.77], [1744.34, 155.57], [1482.21, 2998.55], [5602.89, 718.52], [2601.19, 536.62],
                  [3901.97, 3519.03], [3404.64, 4735.96], [3159.89, 1461.83], [2909.04, 3692.33], [5051.68, 217.17],
                  [2750.33, 1498.77], [2300.66, 4630.88], [1724.63, 4773.81], [849.3, 3403.76], [373.53, 3274.02],
                  [162.58, 830.07], [3545.51, 527.4], [2714.94, 2826.53], [1460.56, 421.79], [1270.76, 1887.69],
                  [4590.2, 4764.17], [1017.31, 3817.17], [1135.87, 2313.44], [223.06, 281.72], [1602.97, 1596.34],
                  [1523.71, 3735.27], [1220.03, 4708.32], [1836.7, 3213.17], [850.0, 1037.88], [3428.56, 174.08],
                  [1951.7, 3872.38], [5461.35, 4267.56], [5629.11, 4708.84], [2966.95, 1893.33], [785.5, 4739.86],
                  [2009.21, 1729.71], [272.93, 3710.73], [1425.18, 3379.12], [1634.81, 2058.78], [4109.44, 2798.43],
                  [371.96, 1241.17], [420.69, 2829.24], [2470.76, 202.28], [3476.96, 4152.14], [1278.86, 1293.07],
                  [3377.24, 880.83], [2202.51, 3419.82], [3040.36, 2278.37], [2366.72, 4212.89], [4385.76, 3802.55],
                  [3724.13, 4443.07], [5500.84, 2783.64], [1524.71, 2570.42], [398.43, 4100.32], [2465.06, 1914.74],
                  [5655.98, 234.82], [2523.43, 3671.84], [2769.23, 4782.2], [1634.34, 1003.21], [4198.21, 4735.72],
                  [3048.19, 3219.07], [5686.79, 1262.43], [3161.37, 2663.3], [2061.56, 2275.08], [4112.04, 1879.63],
                  [2558.88, 2386.11], [3608.08, 2490.56], [4199.14, 4234.67], [3990.27, 800.72], [5672.22, 2031.28],
                  [3550.86, 1916.54], [2171.66, 1181.41], [978.92, 2988.2], [4157.17, 2369.33], [557.13, 660.74],
                  [4194.5, 1331.76], [773.52, 1430.94], [310.58, 4638.59], [4788.8, 4085.01], [2825.91, 998.72],
                  [2963.55, 4200.54], [2974.0, 270.48], [4010.8, 258.35], [313.52, 1823.13], [2093.38, 522.14],
                  [4922.18, 2982.13], [5639.88, 3572.67], [3650.09, 1313.96], [5036.6, 4595.7], [1578.8, 4231.66],
                  [3655.42, 3057.01], [4407.41, 3181.24], [554.62, 2356.13], [5175.43, 701.98], [5152.13, 3526.41],
                  [1124.73, 345.85], [2213.2, 2958.41], [3375.15, 3733.27], [4568.93, 310.02], [966.61, 4228.23]]
        ob_num = len(ob_pos)
        s_num = ob_num - 50
        for i in range(s_num):
            radius = 3 * obstacle_radius
            vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, random.randint(6, 6), is_random=False)
            polygon = Polygon(vertices)
            pos = (polygon.centroid.x, polygon.centroid.y)
            obstacles.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i,
                                          tid='poly' + str(i), vertices=vertices, pos=pos))
        for i in range(s_num, ob_num + 1):
            radius = 5 * obstacle_radius
            if ob_num - 10 < i <= ob_num - 5:  # 倒数第五个为凹形
                radius = 8 * obstacle_radius
                vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, 3, is_random=False)
                vertices.append(ob_pos[i][:2])
            elif ob_num > i > ob_num - 5:  # 最后四个为正方形
                vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, 4, is_random=False)
            elif i <= s_num + 13:
                vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, 3, is_random=False)
            elif s_num + 13 < i <= s_num + 27:
                vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, 4, is_random=False)
            elif i == ob_num:
                vertices = np.array([[4600, 1200], [5200, 1200], [5200, 1300], [4700, 1300],
                                     [4700, 2400], [5200, 2400], [5200, 2500], [4600, 2500]])
            else:
                vertices = gen_polygonal_vertices(ob_pos[i][:2], radius, 5, is_random=False)
            polygon = Polygon(vertices)
            pos = (polygon.centroid.x, polygon.centroid.y)
            obstacles.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i,
                                          tid='poly' + str(i), vertices=vertices, pos=pos))

        return obstacles

    def small_indoor_map(self):
        self.xDim, self.yDim = 6., 5.
        obstacles = []
        ob_pos = [
            [0.3, 0.98, {'shape': 'rect', 'rect': (0.6, 0.5)}],
            [0.0, 0.9, {'shape': 'rect', 'rect': (0.3, 1.0)}],
            [1.8, 0, {'shape': 'rect', 'rect': (0.1, 0.69)}],
            [1.9, 2.7, {'shape': 'concave', 'concave': (0.6, 1.2, 0.1)}],
            [2.95, 0.9, {'shape': 'circle', 'circle': 0.35}],
            [4.3, 0.0, {'shape': 'concave', 'concave': (0.6, 1.1, 0.1)}],
            [1., 4.63, {'shape': 'rect', 'rect': (0.23, 0.37)}],
            [1.6, 1.9, {'shape': 'rect', 'rect': (0.23, 0.1)}],
            [4.6, 3.3, {'shape': 'rect', 'rect': (0.46, 0.1)}],
            [3.6, 3.2, {'shape': 'rect', 'rect': (0.1, 0.23)}],
            # [0.9, 3.1, {'shape': 'rect', 'rect': (0.1, 0.46)}],
            [3.2, 2.3, {'shape': 'rect', 'rect': (0.23, 0.1)}],
            [4.6, 2.3, {'shape': 'rect', 'rect': (0.23, 0.2)}],
            [3.7, 4.54, {'shape': 'rect', 'rect': (0.2, 0.46)}],
        ]
        for i in range(len(ob_pos)):
            vertices = gen_polygon_vertices(ob_pos[i][:2], ob_pos[i][2])
            polygon = Polygon(vertices)
            pos = (polygon.centroid.x, polygon.centroid.y)
            obstacles.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i,
                                          tid='poly' + str(i), vertices=vertices, pos=pos))

        return obstacles

    def small_map(self):
        """
        小范围的二维场景，多边形绘制，50m*30m
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 50, 30
        x, y = self.xDim, self.yDim
        poly_obs = []
        obs_vertices = [self.rect_vertices(0, 0, x, 1), self.rect_vertices(x - 1, 1, x, y - 1),
                        self.rect_vertices(0, y - 1, x, y), self.rect_vertices(0, 1, 1, y - 1),
                        [[20, 0], [21, 0], [21, 15], [10, 15], [10, 14], [20, 14]],
                        [[40, 0], [41, 0], [41, 15], [40, 15]],
                        [[29, 15], [30, 15], [30, 30], [29, 30]]
                        ]
        for i, vertices in enumerate(obs_vertices):
            vertices = np.array(vertices)
            poly_obs.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i, tid='poly' + str(i), vertices=vertices))

        return poly_obs

    def poly_obs_map(self):
        """
        大范围的二维场景，多边形绘制，680m*500m
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 680, 500
        x, y = self.xDim, self.yDim
        poly_obs = []
        obs_vertices = [self.rect_vertices(0, 0, x, 5), self.rect_vertices(x - 5, 5, x, y),
                        self.rect_vertices(0, y - 5, x, y), self.rect_vertices(0, 5, 5, y - 5),
                        [[100, 295], [245, 295], [245, 0], [250, 0], [250, 300], [100, 300]],
                        [[370, 200], [375, 200], [375, 500], [370, 500]],
                        [[495, 0], [500, 0], [500, 400], [495, 400]]
                        ]
        for i, vertices in enumerate(obs_vertices):
            vertices = np.array(vertices)
            poly_obs.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i, tid='poly' + str(i), vertices=vertices))

        return poly_obs

    def symmetrical_map(self):
        """
        大范围的走廊场景，多边形绘制, 800*200
        """

        self.xDim, self.yDim = 800, 200
        x, y = self.xDim, self.yDim
        poly_obs = []
        obs_vertices = [self.rect_vertices(0, 0, x, 5), self.rect_vertices(x - 5, 5, x, y),
                        self.rect_vertices(0, y - 5, x, y), self.rect_vertices(0, 5, 5, y - 5),
                        [[200, 50], [400, 50], [400, 100], [300, 150], [200, 100]],
                        ]
        for i, vertices in enumerate(obs_vertices):
            vertices = np.array(vertices)
            poly_obs.append(PolyObstacle(shape_dict={'shape': 'rect'}, id=i, tid='poly' + str(i), vertices=vertices))

        return poly_obs

    def poly_maze_map(self):
        """
        大范围的二维迷宫场景，多边形绘制, 500m*500m
        Initialize obstacles' positions
        :return: map of obstacles
        """
        self.xDim, self.yDim = 500, 500
        x, y = self.xDim, self.yDim
        poly_obs = []
        obs_vertices = [self.rect_vertices(0, 0, x, 1), self.rect_vertices(x - 1, 1, x, y - 1),
                        self.rect_vertices(0, y - 1, x, y), self.rect_vertices(0, 1, 1, y - 1),
                        [[0, 50], [50, 50], [50, 100], [49, 100], [49, 51], [0, 51]],
                        [[0, 150], [100, 150], [100, 151], [0, 151]],
                        [[0, 200], [100, 200], [100, 201], [51, 201], [51, 250], [100, 250], [100, 251],
                         [51, 251], [51, 450], [50, 450], [50, 201], [0, 201]],
                        [[100, 0], [101, 0], [101, 50], [100, 50]],
                        [[300, 0], [301, 0], [301, 50], [350, 50], [350, 51], [251, 51], [251, 100],
                         [300, 100], [300, 101], [251, 101], [251, 150], [301, 150], [301, 200], [300, 200],
                         [300, 151], [251, 151], [251, 250], [300, 250], [300, 251], [251, 251], [251, 300],
                         [300, 300], [300, 301], [201, 301], [201, 400], [200, 400], [200, 351], [151, 351],
                         [151, 400], [150, 400], [150, 351], [101, 351], [101, 400], [100, 400], [100, 350],
                         [150, 350], [150, 301], [100, 301], [100, 300], [150, 300], [150, 200], [151, 200],
                         [151, 350], [200, 350], [200, 300], [250, 300], [250, 251], [200, 251], [200, 250],
                         [250, 250], [250, 201], [200, 201], [200, 200], [250, 200], [250, 51], [201, 51],
                         [201, 151], [150, 151], [150, 101], [100, 101], [100, 100], [150, 100], [150, 50],
                         [151, 50], [151, 150], [200, 150], [200, 50], [300, 50]],
                        [[100, 450], [201, 450], [201, 500], [200, 500], [200, 451], [100, 451]],
                        [[250, 450], [251, 450], [251, 500], [250, 500]],
                        [[400, 0], [401, 0], [401, 50], [400, 50]],
                        [[450, 0], [451, 0], [451, 101], [350, 101], [350, 100], [450, 100]],
                        [[350, 150], [401, 150], [401, 200], [450, 200], [450, 150], [451, 150], [451, 250],
                         [500, 250], [500, 251], [400, 251], [400, 250], [450, 250], [450, 201], [351, 201],
                         [351, 250], [350, 250], [350, 200], [400, 200], [400, 151], [350, 151]],
                        [[350, 300], [500, 300], [500, 301], [350, 301]],
                        [[250, 350], [500, 350], [500, 351], [250, 351]],
                        [[250, 400], [500, 400], [500, 401], [401, 401], [401, 450], [400, 450], [400, 401],
                         [351, 401], [351, 451], [300, 451], [300, 450], [350, 450], [350, 401], [250, 401]],
                        [[450, 450], [451, 450], [451, 500], [450, 500]],
                        ]

        for i, vertices in enumerate(obs_vertices):
            vertices = np.array(vertices)
            poly_obs.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i, tid='poly' + str(i), vertices=vertices))

        return poly_obs

    def corridor_map(self):
        """
        大范围的走廊场景，多边形绘制, 800*200
        """

        self.xDim, self.yDim = 800, 200
        x, y = self.xDim, self.yDim
        poly_obs = []
        obs_vertices = [self.rect_vertices(0, 0, x, 0.5), self.rect_vertices(x - 0.5, 0.5, x, y),
                        self.rect_vertices(0, y - 0.5, x, y), self.rect_vertices(0, 0.5, 0.5, y - 0.5),
                        [[0, 44], [37, 44], [37, 67], [0, 67]],
                        [[20, 142], [60, 142], [60, 182], [20, 182]],
                        [[73, 43], [104, 43], [104, 103], [73, 103]],
                        [[138, 15], [166, 15], [166, 43], [138, 43]],
                        [[142, 126], [154, 126], [154, 156], [190, 156], [190, 200], [110, 200], [110, 156],
                         [142, 156]],
                        [[183, 63], [221, 63], [221, 101], [258, 101], [258, 136], [232, 136], [232, 111],
                         [183, 111]],
                        [[280, 0], [313, 0], [313, 43], [280, 43]],
                        [[287, 72], [333, 72], [333, 89], [287, 89]],
                        [[345, 184], [357, 184], [357, 200], [345, 200]],
                        [[366, 23], [400, 23], [400, 33], [366, 33]],
                        [[372, 73], [400, 73], [400, 104], [372, 104]],
                        [[444, 32], [460, 32], [460, 66], [444, 66]],
                        [[438, 122], [455, 122], [455, 139], [438, 139]],
                        [[398, 159], [517, 159], [517, 176], [398, 176]],
                        [[544, 31], [581, 31], [581, 47], [544, 47]],
                        [[496, 65], [531, 65], [531, 76], [554, 76], [554, 111], [496, 111]],
                        [[586, 74], [607, 74], [607, 98], [586, 98]],
                        [[629, 38], [646, 38], [646, 59], [629, 59]],
                        [[596, 129], [625, 129], [625, 158], [596, 158]],
                        [[687, 34], [716, 34], [716, 59], [687, 59]],
                        [[684, 89], [710, 89], [710, 138], [684, 138]],
                        [[660, 166], [763, 166], [763, 200], [660, 200]],
                        [[299, 121], [331, 121], [331, 149], [299, 149]],
                        ]
        for i, vertices in enumerate(obs_vertices):
            vertices = np.array(vertices)
            poly_obs.append(PolyObstacle(shape_dict={'shape': 'polygon'}, id=i, tid='poly' + str(i), vertices=vertices))

        return poly_obs

    def read_json(self, name='/corridor_env.json'):
        """
        大范围的走廊场景，栅格绘制, 800*200
        """
        # 从 json 文件中读取数据
        save_dir = os.path.dirname(os.path.realpath(__file__)) + '/map/'
        with open(save_dir + name, 'r') as file:
            data = json.load(file)
        dim = data['dim']
        cmap = data['data']
        self.xDim, self.yDim = dim

        blocks = set()
        for p in cmap:
            blocks.add((p[0], p[1]))
        return blocks

    def write_env_cfg(self, blocks, name='/corridor_env.json'):
        corridor = {'data': [], 'dim': []}
        save_dir = os.path.dirname(os.path.realpath(__file__)) + '/map/'
        for point in blocks:
            corridor['data'].append(point)
        corridor['dim'] = [self.xDim, self.yDim]
        info_str = json.dumps(corridor, indent=4)
        with open(save_dir + name, 'w') as json_file:
            json_file.write(info_str)
        json_file.close()

    def plot_env(self, planner, title="JPS"):
        colors = rcParams['axes.prop_cycle'].by_key()['color']  # 获取默认颜色循环
        fig, ax = plt.subplots()

        path = planner.path
        visited = [] if title[:3] == "GOS" else list(planner.CLOSED)
        s_start = planner.s_start
        s_goal = planner.s_goal

        # plot obstacles
        if self.is_grid_map:
            obs = self.obs_grid
            obs_x = [x[0] for x in obs]
            obs_y = [x[1] for x in obs]
            plt.plot(obs_x, obs_y, "sk", zorder=3)
        else:
            poly_obs = self.poly_obs
            for i in range(len(poly_obs)):
                color = colors[i % len(colors)]
                draw_polygon_2d(ax, poly_obs[i].vertices_pos, fc_color=color, ec_color=color, alpha=1)
                if title[:3] == "GOS":
                    draw_polygon_2d(ax, planner.inflated_obs[i].vertices_pos, fc_color='none', ec_color=color,
                                    alpha=0.9)

        px = [x[0] for x in path]
        py = [x[1] for x in path]
        ax.scatter(px[:], py[:], marker='o', color='red')
        plt.plot(px, py, linewidth=2, color='blue', zorder=3)
        plt.plot(s_start[0], s_start[1], "bs", zorder=3)
        plt.plot(s_goal[0], s_goal[1], "rs", zorder=3)

        plt.axis("equal")
        plt.title(title)
        plt.show()

    def plot_paths(self, paths, poly_obs):
        colors = rcParams['axes.prop_cycle'].by_key()['color']  # 获取默认颜色循环
        fig, ax = plt.subplots()

        # 将路径分解为X和Y坐标
        x1, y1 = zip(*paths[0])
        x2, y2 = zip(*paths[1])
        x3, y3 = zip(*paths[2])

        # plot obstacles
        if self.is_grid_map:
            obs = self.obs_grid
            obs_x = [x[0] for x in obs]
            obs_y = [x[1] for x in obs]
            plt.plot(obs_x, obs_y, "sk", zorder=3)
        else:
            for i in range(len(poly_obs)):
                color = colors[i % len(colors)]
                draw_polygon_2d(ax, poly_obs[i].vertices_pos, fc_color=color, ec_color=color, alpha=1)

        ax.plot(x1, y1, label='GOS', color='blue', linewidth=3)
        ax.plot(x2, y2, label='JPS', color='red', linewidth=3)
        ax.plot(x3, y3, label='A*', color='green', linewidth=3)
        # ax.scatter(x2[:], y2[:], marker='o', color='blue')
        # ax.scatter(x1[:], y1[:], marker='o', color='red')
        # ax.scatter(x3[:], y3[:], marker='o', color='green')
        plt.plot(x1[0], y1[0], "bs", zorder=3)
        plt.plot(x1[-1], y1[-1], "rs", zorder=3)
        ax.legend()
        ax.set_title('Global Path Planning')
        ax.set_xlabel('X (m)')
        ax.set_ylabel('Y (m)')
        plt.axis("equal")
        plt.show()


def gen_polygonal_vertices(center, radius, num_vertice, is_random=False):
    regular_polygon = True if num_vertice <= 12 else False  # 多边形边数少于6则默认为正多边形，否则为随机多边形

    if regular_polygon:
        vertices = []
        s_theta = np.random.uniform(0, 360 / num_vertice) if is_random else 360 / num_vertice / 2
        for theta in [np.deg2rad(i * (360 / num_vertice) + s_theta) for i in range(num_vertice)]:
            vertice = center + radius * np.array([np.cos(theta), np.sin(theta)])
            vertices.append(vertice)
    else:
        points_num = int(2 * np.pi * radius / 30.0)
        all_vertices = []
        for angular in [np.deg2rad(i * (360 / points_num)) for i in range(points_num)]:
            vertice = center + radius * np.array([np.cos(angular), np.sin(angular)])
            all_vertices.append([vertice, angular])
        vertices_theta = random.sample(all_vertices, num_vertice)
        vertices_theta = sorted(vertices_theta, key=lambda x: x[1])
        vertices = []
        for vertice in vertices_theta:
            vertices.append(vertice[0])

    return vertices


def gen_polygon_vertices(start_pos, shape_dict):
    vertices = []
    x, y = start_pos[0], start_pos[1]
    shape = shape_dict['shape']
    if shape == 'rect':
        length, width = shape_dict['rect']
        vertices = np.array([[x, y], [x + length, y], [x + length, y + width], [x, y + width]])
    elif shape == 'circle':
        radius = shape_dict['circle']
        vertices = gen_polygonal_vertices(start_pos, radius, 8, is_random=False)
    elif shape == 'concave':
        w, cave_l, con_w = shape_dict['concave']  # 实物实验设为0.6, 1.2, 0.1
        vertices = np.array([[x, y], [x + w, y], [x + w, y + con_w], [x + con_w, y + con_w],
                             [x + con_w, y + cave_l], [x + w, y + cave_l], [x + w, y + cave_l + con_w],
                             [x, y + cave_l + con_w]])
    return vertices


def plot_env(obs, is_grid=False):
    fig, ax = plt.subplots()
    colors = rcParams['axes.prop_cycle'].by_key()['color']  # 获取默认颜色循环

    # plot obstacles
    if not is_grid:
        for i in range(len(obs)):
            color = colors[i % len(colors)]
            draw_polygon_2d(ax, obs[i].vertices_pos, fc_color=color, ec_color=color, alpha=1)
    else:
        obs_x = [x[0] for x in obs]
        obs_y = [x[1] for x in obs]
        plt.plot(obs_x, obs_y, "sk", zorder=3)
    # envs1 = Env(envs_type[0], is_grid_map=True)
    # obs_grid = envs1.obs_grid
    # obs_x = [x[0] for x in obs_grid]
    # obs_y = [x[1] for x in obs_grid]
    # plt.plot(obs_x, obs_y, "sk", zorder=3)

    plt.axis("equal")
    plt.show()


if __name__ == "__main__":
    envs_type = {0: "small", 1: "large_maze", 2: "large", 3: "large_corridor", 4: "symmetric",
                 5: "large_obs", 6: "small_indoor"}
    envs = Env(envs_type[3], is_grid_map=False)
    polygon_obs = envs.poly_obs
    plot_env(polygon_obs, False)
